/*! 
 * Implementation file for for the rigid body world structure.
 *
 * \author Devis Rossini
 * \date 16 April 2012
 */
#include "PhxScene.h"

#include "PhxCollisionAlgorithm.h"
#include "PhxCollisionAlgorithmManager.h"

#include "PhxCollisionDetector.h"

#include <algorithm> // Are you sure, Devis? OMG! o_O
#include <memory>
#include <cassert>

using namespace phx;

// TODO: Rewrite this stuff so seriously...please :( This use of STL smells! o_O

Scene* Scene::CreateScene(	unsigned iContacts,
							unsigned iIterations,
							real iDefaultFriction,
							real iDefaultRestitution,
							real iDefaultTolerance,
							real iResolverPositionEpsilon, 
							real iResolverVelocityEpsilon,
							real iIntegrationStep)
{
	return new Scene(iContacts, iIterations, iDefaultFriction, iDefaultRestitution, iDefaultTolerance, iResolverPositionEpsilon, iResolverVelocityEpsilon, iIntegrationStep);
}

void Scene::DestroyScene(Scene* iScene)
{
	delete iScene;
}

void Scene::AddPrimitive(CollisionPrimitive* iPrimitive)
{
	assert(iPrimitive);
	mPrimitives.push_back(iPrimitive);
}

CollisionPrimitive* Scene::CreatePrimitive(const CollisionPrimitiveDescriptor& iDesc)
{
	CollisionPrimitive* newPrimitive = NULL;

	ShapeTypes shapeType = iDesc.GetShapeType();
	switch(shapeType)
	{
	case SPHERE_SHAPE:
		{
			// We have to create a sphere.
			const CollisionSphereDescriptor& sphereDesc = static_cast<const CollisionSphereDescriptor&>(iDesc);
			newPrimitive = new CollisionSphere(sphereDesc);

			break;
		}

	case PLANE_SHAPE:
		{
			// We have to create a plane.
			const CollisionPlaneDescriptor& planeDesc = static_cast<const CollisionPlaneDescriptor&>(iDesc);
			newPrimitive = new CollisionPlane(planeDesc);

			break;
		}

	case BOX_SHAPE:
		{
			// We have to create a box.
			const CollisionBoxDescriptor& boxDesc = static_cast<const CollisionBoxDescriptor&>(iDesc);
			newPrimitive = new CollisionBox(boxDesc);

			break;
		}

	case TERRAIN_SHAPE:
		{
			// We have to create a terrain.
			const CollisionTerrainDescriptor& terrainDesc = static_cast<const CollisionTerrainDescriptor&>(iDesc);
			newPrimitive = new CollisionTerrain(terrainDesc);

			break;
		}

	default:

		// "You shall not pass!" (Gandalf, LOTR)
		assert(false);
		break;
	}

	assert(newPrimitive);

	mPrimitives.push_back(newPrimitive);

	return newPrimitive;
}

void Scene::AddSphere(CollisionSphere* iSphere)
{
	assert(iSphere);
	assert(iSphere->GetShapeType() == SPHERE_SHAPE);
	mPrimitives.push_back(iSphere);
}

CollisionSphere* Scene::CreateSphere(const CollisionSphereDescriptor& iSphereDesc)
{
	assert(iSphereDesc.GetShapeType() == SPHERE_SHAPE);

	CollisionSphere* newSphere = new CollisionSphere(iSphereDesc);
	mPrimitives.push_back(newSphere);

	return newSphere;
}

void Scene::AddPlane(CollisionPlane* iPlane)
{
	assert(iPlane);
	assert(iPlane->GetShapeType() == PLANE_SHAPE);
	mPrimitives.push_back(iPlane);
}

CollisionPlane* Scene::CreatePlane(const CollisionPlaneDescriptor& iPlaneDesc)
{
	assert(iPlaneDesc.GetShapeType() == PLANE_SHAPE);

	CollisionPlane* newPlane = new CollisionPlane(iPlaneDesc);
	mPrimitives.push_back(newPlane);

	return newPlane;
}

void Scene::AddTerrain(CollisionTerrain* iTerrain)
{
	assert(iTerrain);
	assert(iTerrain->GetShapeType() == TERRAIN_SHAPE);
	mPrimitives.push_back(iTerrain);
}

CollisionTerrain* Scene::CreateTerrain(const CollisionTerrainDescriptor& iTerrainDesc)
{
	assert(iTerrainDesc.GetShapeType() == TERRAIN_SHAPE);

	CollisionTerrain* newTerrain = new CollisionTerrain(iTerrainDesc);
	mPrimitives.push_back(newTerrain);

	return newTerrain;
}

void Scene::AddBox(CollisionBox* iBox)
{
	assert(iBox);
	assert(iBox->GetShapeType() == BOX_SHAPE);
	mPrimitives.push_back(iBox);
}

CollisionBox* Scene::CreateBox(const CollisionBoxDescriptor& iBoxDesc)
{
	assert(iBoxDesc.GetShapeType() == BOX_SHAPE);

	CollisionBox* newBox = new CollisionBox(iBoxDesc);
	mPrimitives.push_back(newBox);

	return newBox;
}

void Scene::RemovePrimitive(CollisionPrimitive* iPrimitive)
{
	// Find the primitive to remove.
	CollisionPrimitives::iterator it = std::find(mPrimitives.begin(), mPrimitives.end(), iPrimitive);

	// Remove it!
	mPrimitives.erase(it);
}

void Scene::DestroyPrimitive(CollisionPrimitive* iPrimitive)
{
	RemovePrimitive(iPrimitive);
	delete iPrimitive;
}

Scene::Scene(unsigned iContacts, unsigned iIterations, real iDefaultFriction, real iDefaultRestitution, real iDefaultTolerance, real iResolverPositionEpsilon, real iResolverVelocityEpsilon, real iIntegrationStep)
	: mIntegrationStep(iIntegrationStep), mPrevTime(0), mMaxContacts(iContacts), mCalculateIterations(iIterations == 0), mResolver(iIterations, iResolverVelocityEpsilon, iResolverPositionEpsilon)
{
	// Set the collision data.
	mCollisionData.mContactArray	= new Contact[mMaxContacts];
	mCollisionData.mFriction		= iDefaultFriction;
	mCollisionData.mRestitution		= iDefaultRestitution;
	mCollisionData.mTolerance		= iDefaultTolerance;

	mCollisionData.Reset(mMaxContacts);

	InitAlgorithmManager();
}

Scene::~Scene()
{
	delete mCollisionData.mContactArray;

	// Destroy all primitives.
	for (size_t i = 0; i < mPrimitives.size(); ++i)
	{
		delete mPrimitives.at(i);
	}
}

void Scene::AddForceGeneator(RigidBody* iBody, ForceGenerator* iForceGenerator)
{
	// TODO: Assert that iBody is a valid pointer to a rigid body associated to the scene.
	mForceRegistry.Add(iBody, iForceGenerator);
}

void Scene::AddForceGeneator(const ForceRegistration& iForceRegistration)
{
	// TODO: Assert that iForceRegistration.body is a valid pointer to a rigid body associated to the scene.
	mForceRegistry.Add(iForceRegistration);
}

void Scene::StartFrame()
{
	for (CollisionPrimitives::iterator it = mPrimitives.begin(); it != mPrimitives.end(); ++it)
	{
		CollisionPrimitive* primitive = *it;

		primitive->GetBody()->ClearAccumulators();
		primitive->GetBody()->CalculateDerivedData();
	}
}

unsigned Scene::GenerateContacts()
{
	// Reset the collision array.
	mCollisionData.Reset(mMaxContacts);
	
	unsigned collisionCounter = 0;

	// Check each pair of primitives associated with the scene.
	for (CollisionPrimitives::iterator it1 = mPrimitives.begin(); 
			it1 != mPrimitives.end(); ++it1)
	{
		for (CollisionPrimitives::iterator it2 = it1 + 1; 
				it2 != mPrimitives.end(); ++it2) 
		{
			CollisionPrimitive* primitiveOne = (*it1);
			CollisionPrimitive* primitiveTwo = (*it2);

			ShapeTypes type1 = primitiveOne->GetShapeType();
			ShapeTypes type2 = primitiveTwo->GetShapeType();

			Algorithm collisionAlgorithm = mAlgorithmManager.GetAlgorithm(type1, type2);

			if (collisionAlgorithm)
			{
				if (!mCollisionData.HasMoreContacts()) return collisionCounter;
				collisionCounter += collisionAlgorithm(primitiveOne, primitiveTwo, &mCollisionData);
			}
		}
	}

	// TODO: Generate contacts from the contact generators. Toc toc..do you remember it?

	return collisionCounter;
}

void Scene::RunPhysics(real iDuration)
{
	real elapsedTime = mPrevTime + iDuration;

	while (elapsedTime > mIntegrationStep)
	{
		StartFrame();
		Integrate(mIntegrationStep);
		elapsedTime -= mIntegrationStep;
	}

	mPrevTime = elapsedTime;
}

void Scene::RunPhysicsWithVariableStep(real iDuration)
{
	StartFrame();
	Integrate(iDuration);
}

void Scene::SetDefaultFriction(real iFriction)
{
	mCollisionData.mFriction = iFriction;
}

const real Scene::GetDefaultFriction() const
{
	return mCollisionData.mFriction;
}

void Scene::SetDefaultRestitution(real iRestitution)
{
	mCollisionData.mRestitution = iRestitution;
}

const real Scene::GetDefaultRestitution() const
{
	return mCollisionData.mRestitution;
}

void Scene::SetDefaultTolerance(real iTolerance)
{
	mCollisionData.mTolerance = iTolerance;
}

const real Scene::GetDefaultTolerance() const
{
	return mCollisionData.mTolerance;
}

void Scene::InitAlgorithmManager()
{
	// Sphere.
	mAlgorithmManager.RegisterAlgorithm(SPHERE_SHAPE, SPHERE_SHAPE,	&CollisionDetector::SphereAndSphere);
	mAlgorithmManager.RegisterAlgorithm(SPHERE_SHAPE, PLANE_SHAPE, &CollisionDetector::SphereAndPlane);
	mAlgorithmManager.RegisterAlgorithm(SPHERE_SHAPE, TERRAIN_SHAPE, &CollisionDetector::SphereAndTerrain);
	mAlgorithmManager.RegisterAlgorithm(SPHERE_SHAPE, BOX_SHAPE, &CollisionDetector::SphereAndBox);

	// Plane.
	mAlgorithmManager.RegisterAlgorithm(PLANE_SHAPE, SPHERE_SHAPE, &CollisionDetector::PlaneAndSphere);
	mAlgorithmManager.RegisterAlgorithm(PLANE_SHAPE, PLANE_SHAPE, &CollisionDetector::PlaneAndPlane);
	mAlgorithmManager.RegisterAlgorithm(PLANE_SHAPE, TERRAIN_SHAPE, &CollisionDetector::PlaneAndTerrain);
	mAlgorithmManager.RegisterAlgorithm(PLANE_SHAPE, BOX_SHAPE, &CollisionDetector::PlaneAndBox);

	// Terrain.
	mAlgorithmManager.RegisterAlgorithm(TERRAIN_SHAPE, SPHERE_SHAPE, &CollisionDetector::TerrainAndSphere);
	mAlgorithmManager.RegisterAlgorithm(TERRAIN_SHAPE, PLANE_SHAPE, &CollisionDetector::TerrainAndPlane);
	mAlgorithmManager.RegisterAlgorithm(TERRAIN_SHAPE, TERRAIN_SHAPE, &CollisionDetector::TerrainAndTerrain);
	mAlgorithmManager.RegisterAlgorithm(TERRAIN_SHAPE, BOX_SHAPE, &CollisionDetector::TerrainAndBox);

	// Box.
	mAlgorithmManager.RegisterAlgorithm(BOX_SHAPE, SPHERE_SHAPE, &CollisionDetector::BoxAndSphere);
	mAlgorithmManager.RegisterAlgorithm(BOX_SHAPE, PLANE_SHAPE, &CollisionDetector::BoxAndPlane);
	mAlgorithmManager.RegisterAlgorithm(BOX_SHAPE, TERRAIN_SHAPE, &CollisionDetector::BoxAndTerrain);
	mAlgorithmManager.RegisterAlgorithm(BOX_SHAPE, BOX_SHAPE, &CollisionDetector::BoxAndBox);
}

void Scene::Integrate(real iDuration)
{
	// First apply the force generators.
	mForceRegistry.UpdateForces(iDuration);

	// Then integrate the objects.
	for (CollisionPrimitives::iterator it = mPrimitives.begin(); it != mPrimitives.end(); ++it)
	{
		CollisionPrimitive* primitive = (*it);

		primitive->GetBody()->Integrate(iDuration);
		primitive->CalculateTransform();
	}

	// Generate additional contacts.
	unsigned usedContacts = GenerateContacts();

	// Process the contacts.
	if (mCalculateIterations) mResolver.SetIterations(usedContacts * 4);
	mResolver.ResolveContacts(mCollisionData.mContactArray, mCollisionData.mContactCount, iDuration);
}